//
// Power By MrC
//

#ifndef ONEBLOW_FLC_HAL_MPU60X0_H
#define ONEBLOW_FLC_HAL_MPU60X0_H

#define MPU_ADD 0xD0	// AD0 low
//#define MPU6050_ADDRESS 0xD1	// AD0 high
#include <math.h>

class MPU_60x0
{
protected:

    uint8_t Smplrt_div;

    double  Kp = 100.0;
    double Ki = 0.002f;
    double halfT = 0.001f;

    double q0 = 1, q1 = 0, q2 = 0, q3 = 0;
    double exInt = 0, eyInt = 0, ezInt = 0;


public:


    enum GYRO_RANGE { BPS_250, BPS_500, BPS_1000, BPS_2000 } GyrR;

    enum ACCE_RANGE { ACC_2G, ACC_4G, ACC_8G, ACC_16G } AccR;

    struct MPU_Data
    {
        int16_t gyro_x_raw,gyro_y_raw,gyro_z_raw,acce_x_raw,acce_y_raw,acce_z_raw;
        double Yaw,Pitch,Roll;
    } data;

    MPU_60x0(I2C_HandleTypeDef *mpu_i2c_interface,GYRO_RANGE GR,ACCE_RANGE AR):
            i2c(mpu_i2c_interface),AccR(AR),GyrR(GR) {TimeOut = 5; DlpfSW = DLPF_OFF;}

    I2C_HandleTypeDef *i2c;
    uint32_t TimeOut,SampleRate;

    enum MPU_STATE
    {
        MPU_OK,
        MPU_ERROR
    };

    enum DLPF_SW { DLPF_OFF, DLPF_1, DLPF_2, DLPF_3, DLPF_4,   DLPF_5, DLPF_6 } DlpfSW;
    MPU_60x0() { TimeOut = 5; DlpfSW = DLPF_OFF; }
    enum AXIS_SW { x_axi, y_axi, z_axi };
    MPU_60x0(I2C_HandleTypeDef *mpu_i2c_interface) : i2c(mpu_i2c_interface)
    {
        TimeOut=5;
        DlpfSW = DLPF_OFF;
    }


    MPU_STATE Get_Gyro_OneAXIS_RawData(AXIS_SW AS)
    {
        uint16_t gyroData = 0;
        int16_t gyroData_flag;
        uint8_t Data_t = 0xff;

        switch (AS)
        {
            case x_axi:
                if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x43,1,&Data_t,1,TimeOut)!=HAL_OK)
                    return MPU_ERROR;
                gyroData += ((uint16_t)Data_t & 0x00ff);
                gyroData = gyroData << 8;
                if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x44,1,&Data_t,1,TimeOut)!=HAL_OK)
                    return MPU_ERROR;
                gyroData += ((uint16_t)Data_t & 0x00ff);
                if ((gyroData & 0x8000) == 0x8000)
                {
                    gyroData &= 0x7fff;
                    gyroData_flag -=(int16_t)gyroData_flag;
                } else
                {
                    gyroData_flag = (int16_t)gyroData;
                }
                this->data.gyro_z_raw = gyroData_flag;
                return MPU_OK;

            case y_axi:
                if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x45,1,&Data_t,1,TimeOut)!=HAL_OK)
                    return MPU_ERROR;
                gyroData += ((uint16_t)Data_t & 0x00ff);
                gyroData = gyroData << 8;
                if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x46,1,&Data_t,1,TimeOut)!=HAL_OK)
                    return MPU_ERROR;
                gyroData += ((uint16_t)Data_t & 0x00ff);
                if ((gyroData & 0x8000) == 0x8000)
                {
                    gyroData &= 0x7fff;
                    gyroData_flag -=(int16_t)gyroData_flag;
                } else
                {
                    gyroData_flag = (int16_t)gyroData;
                }
                this->data.gyro_z_raw = gyroData_flag;
                return MPU_OK;

            case z_axi:
                if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x47,1,&Data_t,1,TimeOut)!=HAL_OK)
                    return MPU_ERROR;
                gyroData += ((uint16_t)Data_t & 0x00ff);
                gyroData = gyroData << 8;
                if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x48,1,&Data_t,1,TimeOut)!=HAL_OK)
                    return MPU_ERROR;
                gyroData += ((uint16_t)Data_t & 0x00ff);
                if ((gyroData & 0x8000) == 0x8000)
                {
                    gyroData &= 0x7fff;
                    gyroData_flag -=(int16_t)gyroData_flag;
                } else
                {
                    gyroData_flag = (int16_t)gyroData;
                }
                this->data.gyro_z_raw = gyroData_flag;
                return MPU_OK;

            default: return MPU_ERROR;
        }
    }

    MPU_STATE Get_Gyro_AllAXIS_RawData(void)
    {
        uint16_t gyroData = 0;
        int16_t gyroData_flag;
        uint8_t Data_t = 0xff;

        if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x43,1,&Data_t,1,TimeOut)!=HAL_OK)
            return MPU_ERROR;
        gyroData += ((uint16_t)Data_t & 0x00FF);
        gyroData = gyroData << 8;
        if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x44,1,&Data_t,1,TimeOut)!=HAL_OK)
            return MPU_ERROR;
        gyroData += ((uint16_t)Data_t & 0x00ff);
        if ((gyroData & 0x8000) == 0x8000)
        {
            gyroData &= 0x7fff;
            gyroData_flag -=(int16_t)gyroData_flag;
        } else
        {
            gyroData_flag = (int16_t)gyroData;
        }
        this->data.gyro_x_raw = gyroData_flag;

        gyroData = 0;
        gyroData_flag =0;

        if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x45,1,&Data_t,1,TimeOut)!=HAL_OK)
            return MPU_ERROR;
        gyroData += ((uint16_t)Data_t & 0x00ff);
        gyroData = gyroData << 8;
        if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x46,1,&Data_t,1,TimeOut)!=HAL_OK)
            return MPU_ERROR;
        gyroData += ((uint16_t)Data_t & 0x00ff);
        if ((gyroData & 0x8000) == 0x8000)
        {
            gyroData &= 0x7fff;
            gyroData_flag -=(int16_t)gyroData_flag;
        } else
        {
            gyroData_flag = (int16_t)gyroData;
        }
        this->data.gyro_y_raw = gyroData_flag;

        gyroData = 0;
        gyroData_flag =0;

        if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x47,1,&Data_t,1,TimeOut)!=HAL_OK)
            return MPU_ERROR;
        gyroData += ((uint16_t)Data_t & 0x00ff);
        gyroData = gyroData << 8;
        if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x48,1,&Data_t,1,TimeOut)!=HAL_OK)
            return MPU_ERROR;
        gyroData += ((uint16_t)Data_t & 0x00ff);
        if ((gyroData & 0x8000) == 0x8000)
        {
            gyroData &= 0x7fff;
            gyroData_flag -=(int16_t)gyroData_flag;
        } else
        {
            gyroData_flag = (int16_t)gyroData;
        }
        this->data.gyro_z_raw = gyroData_flag;

        return MPU_OK;
    }

    MPU_STATE Get_Acce_OneAXIS_RawData(AXIS_SW AS)
    {
        uint16_t acceData = 0;
        int16_t acceData_flag = 0;
        uint8_t Data_t = 0;

        switch (AS)
        {
            case x_axi:
                if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x3B,1,&Data_t,1,TimeOut)!=HAL_OK)
                    return MPU_ERROR;
                acceData += ((uint16_t)Data_t & 0x00ff);
                acceData = acceData << 8;
                if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x3C,1,&Data_t,1,TimeOut)!=HAL_OK)
                    return MPU_ERROR;
                acceData += ((uint16_t)Data_t & 0x00FF);
                if ((acceData & 0x8000) == 0x8000)
                {
                    acceData &= 0x7fff;
                    acceData_flag -=(int16_t)acceData;
                } else
                {
                    acceData_flag = (int16_t)acceData;
                }
                this->data.acce_z_raw = acceData_flag;
                return MPU_OK;

            case y_axi:
                if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x3D,1,&Data_t,1,TimeOut)!=HAL_OK)
                    return MPU_ERROR;
                acceData += ((uint16_t)Data_t & 0x00ff);
                acceData = acceData << 8;
                if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x3E,1,&Data_t,1,TimeOut)!=HAL_OK)
                    return MPU_ERROR;
                acceData += ((uint16_t)Data_t & 0x00FF);
                if ((acceData & 0x8000) == 0x8000)
                {
                    acceData &= 0x7fff;
                    acceData_flag -=(int16_t)acceData;
                } else
                {
                    acceData_flag = (int16_t)acceData;
                }
                this->data.acce_z_raw = acceData_flag;
                return MPU_OK;

            case z_axi:
                if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x3F,1,&Data_t,1,TimeOut)!=HAL_OK)
                    return MPU_ERROR;
                acceData += ((uint16_t)Data_t & 0x00ff);
                acceData = acceData << 8;
                if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x40,1,&Data_t,1,TimeOut)!=HAL_OK)
                    return MPU_ERROR;
                acceData += ((uint16_t)Data_t & 0x00FF);
                if ((acceData & 0x8000) == 0x8000)
                {
                    acceData &= 0x7fff;
                    acceData_flag -=(int16_t)acceData;
                } else
                {
                    acceData_flag = (int16_t)acceData;
                }
                this->data.acce_z_raw = acceData_flag;
                return MPU_OK;

            default: return MPU_ERROR;
        }
    }

    MPU_STATE Get_Acce_AllAXIS_RawData(void)
    {
        uint16_t acceData = 0;
        int16_t acceData_flag = 0;
        uint8_t Data_t = 0;

        if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x3B,1,&Data_t,1,TimeOut)!=HAL_OK)
            return MPU_ERROR;
        acceData += ((uint16_t)Data_t & 0x00FF);
        acceData = acceData << 8;
        if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x3C,1,&Data_t,1,TimeOut)!=HAL_OK)
            return MPU_ERROR;
        acceData += ((uint16_t)Data_t & 0x00FF);
        if ((acceData & 0x8000) == 0x8000)
        {
            acceData &= 0x7fff;
            acceData_flag -=(int16_t)acceData;
        } else
        {
            acceData_flag = (int16_t)acceData;
        }

        this->data.acce_x_raw = acceData_flag;

        acceData_flag = 0;
        acceData = 0;

        if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x3D,1,&Data_t,1,TimeOut)!=HAL_OK)
            return MPU_ERROR;
        acceData += ((uint16_t)Data_t & 0x00FF);
        acceData = acceData << 8;
        if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x3E,1,&Data_t,1,TimeOut)!=HAL_OK)
            return MPU_ERROR;

        acceData += ((uint16_t)Data_t & 0x00FF);
        if ((acceData & 0x8000) == 0x8000)
        {
            acceData &= 0x7fff;
            acceData_flag -=(int16_t)acceData;
        } else
        {
            acceData_flag = (int16_t)acceData;
        }


        this->data.acce_y_raw = acceData_flag;

        acceData = 0;
        acceData_flag = 0;

        if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x3F,1,&Data_t,1,TimeOut)!=HAL_OK)
            return MPU_ERROR;
        acceData += ((uint16_t)Data_t & 0x00FF);
        acceData = acceData << 8;
        if (HAL_I2C_Mem_Read(i2c,MPU_ADD,0x40,1,&Data_t,1,TimeOut)!=HAL_OK)
            return MPU_ERROR;

        acceData += ((uint16_t)Data_t & 0x00FF);
        if ((acceData & 0x8000) == 0x8000)
        {
            acceData &= 0x7fff;
            acceData_flag -=(int16_t)acceData;
        } else
        {
            acceData_flag = (int16_t)acceData;
        }
        this->data.acce_z_raw = acceData_flag;

        return MPU_OK;
    }

    double getAcceXdata(void)
    {
        double temp;
        switch (AccR)
        {
            case ACC_2G:
                temp = (double)data.acce_x_raw / 32767 * 9.8;
                return temp;

            case ACC_4G:
                temp = (double)data.acce_x_raw / 16384 * 9.8;
                return temp;

            case ACC_8G:
                temp = (double)data.acce_x_raw / 8192 * 9.8;
                return temp;

            case ACC_16G:
                temp = (double)data.acce_x_raw / 4096 * 9.8;
                return temp;
        }

        return 0;
    }

    double getAcceYdata(void)
    {
        double temp;
        switch (AccR)
        {
            case ACC_2G:
                temp = (double)data.acce_y_raw / 32767 * 9.8;
                return temp;

            case ACC_4G:
                temp = (double)data.acce_y_raw / 16384 * 9.8;
                return temp;

            case ACC_8G:
                temp = (double)data.acce_y_raw / 8192 * 9.8;
                return temp;

            case ACC_16G:
                temp = (double)data.acce_y_raw / 4096 * 9.8;
                return temp;
        }


    }

    double getAcceZdata(void)
    {
        double temp;
        switch (AccR)
        {
            case ACC_2G:
                temp = (double)data.acce_z_raw / 32767 * 9.8;
                return temp;

            case ACC_4G:
                temp = (double)data.acce_z_raw / 16384 * 9.8;
                return temp;

            case ACC_8G:
                temp = (double)data.acce_z_raw / 8192 * 9.8;
                return temp;

            case ACC_16G:
                temp = (double)data.acce_z_raw / 4096 * 9.8;
                return temp;
        }

        return 0;
    }

    double getGyroXdata(void)
    {
        double temp;
        switch (GyrR)
        {
            case BPS_250:
                temp =  (double)data.gyro_x_raw / 262;
                return temp;

            case BPS_500:
                temp =  (double)data.gyro_x_raw / 161;
                return temp;

            case BPS_1000:
                temp =  (double)data.gyro_x_raw / 65.5;
                return temp;

            case BPS_2000:
                temp =  (double)data.gyro_x_raw / 32.75;
                return temp;
        }

        return 0;
    }

    double getGyroYdata(void)
    {
        double temp;
        switch (GyrR)
        {
            case BPS_250:
                temp =  (double)data.gyro_y_raw / 262;
                return temp;

            case BPS_500:
                temp =  (double)data.gyro_y_raw / 161;
                return temp;

            case BPS_1000:
                temp =  (double)data.gyro_y_raw / 65.5;
                return temp;

            case BPS_2000:
                temp =  (double)data.gyro_y_raw / 32.75;
                return temp;
        }

        return 0;
    }

    double getGyroZdata(void)
    {
        double temp;
        switch (GyrR)
        {
            case BPS_250:
                temp =  (double)data.gyro_z_raw / 262;
                return temp;

            case BPS_500:
                temp =  (double)data.gyro_z_raw / 161;
                return temp;

            case BPS_1000:
                temp =  (double)data.gyro_z_raw / 65.5;
                return temp;

            case BPS_2000:
                temp =  (double)data.gyro_z_raw / 32.75;
                return temp;
        }

        return 0;
    }




    MPU_STATE MPU_Set_Power_Mangement(bool EnableSleep = false ,bool EnabletempSenor = true,bool Rest = true)
    {
        uint8_t order = 0x00;

        if (Rest)
        {
            if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x6B,1,&order,1,TimeOut) != HAL_OK)
                return MPU_ERROR;

            HAL_Delay(50);
        }


        if (EnableSleep)
            order |= 0x08;

        if (!EnabletempSenor)
            order |= 0x40;

        if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x6B,1,&order,1,TimeOut) == HAL_OK)
            return MPU_OK;

        return MPU_ERROR;
    }

    MPU_STATE MPU_Set_Sample_Divider(uint8_t SMPLRT_DIV = 99)
    {
        this->Smplrt_div = SMPLRT_DIV;

        if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x19,1,&SMPLRT_DIV,1,TimeOut) == HAL_OK)
            return MPU_OK;
        return MPU_ERROR;
    }

    MPU_STATE MPU_FIFO_ALL_DIS()
    {
        uint8_t order = 0x00;
        if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x23,1,&order,1,TimeOut) == HAL_OK)
            return MPU_OK;
        return MPU_ERROR;
    };



    MPU_STATE MPU_Set_Dlpf_LowpassFilter(DLPF_SW DS = DLPF_OFF)
    {
        DlpfSW = DS;
        uint8_t *order = new uint8_t;

        switch (DlpfSW)
        {
            case DLPF_OFF:
                *order = 0x00;
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1A,1,order,1,TimeOut)== HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;
            case DLPF_1:
                *order = 0x01;
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1A,1,order,1,TimeOut)== HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;
            case DLPF_2:
                *order = 0x02;
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1A,1,order,1,TimeOut)== HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;

            case DLPF_3:
                *order = 0x03;
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1A,1,order,1,TimeOut)== HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;

            case DLPF_4:
                *order = 0x04;
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1A,1,order,1,TimeOut)== HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;

            case DLPF_5:
                *order = 0x05;
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1A,1,order,1,TimeOut)== HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;

            case DLPF_6:
                *order = 0x06;
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1A,1,order,1,TimeOut)== HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;
            default:  return MPU_ERROR;
        }
    }

    uint32_t getSampleRate(void)
    {
        if (DlpfSW == DLPF_OFF)
            this->SampleRate = 8000 / (this->Smplrt_div + 1);
        else { this->SampleRate = 1000 / (this->Smplrt_div + 1); }

        return this->SampleRate;
    }

    MPU_STATE MPU_Set_AG_Range(ACCE_RANGE AR = ACC_2G,GYRO_RANGE GR = BPS_1000)
    {
        if (MPU_Set_Acce_Range(AR) == MPU_OK && MPU_Set_Gyro_Range(GR) == MPU_OK)
            return MPU_OK;
        return MPU_ERROR;
    }



    MPU_STATE MPU_Set_Gyro_Range(GYRO_RANGE GR = BPS_1000)
    {
        uint8_t order = 0xE0;
        GyrR = GR;

        switch (GR)
        {
            case BPS_250:
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1B,1,&order,1,TimeOut) == HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;

            case BPS_500:
                order = 0xE8;
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1B,1,&order,1,TimeOut) == HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;

            case BPS_1000:
                order = 0xF0;
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1B,1,&order,1,TimeOut) == HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;

            case BPS_2000:
                order = 0xF8;
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1B,1,&order,1,TimeOut) == HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;
            default:  return MPU_ERROR;
        }
    }

    MPU_STATE MPU_Set_Acce_Range(ACCE_RANGE AR = ACC_2G)
    {
        uint8_t order = 0xE0;
        AccR = AR;

        switch (AR)
        {
            case ACC_2G:
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1C,1,&order,1,TimeOut) == HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;

            case ACC_4G:
                order = 0xE8;
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1C,1,&order,1,TimeOut) == HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;

            case ACC_8G:
                order = 0xF0;
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1C,1,&order,1,TimeOut) == HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;

            case ACC_16G:
                order = 0xF8;
                if (HAL_I2C_Mem_Write(i2c,MPU_ADD,0x1C,1,&order,1,TimeOut) == HAL_OK)
                    return MPU_OK;
                return MPU_ERROR;
            default:  return MPU_ERROR;
        }
    }


    MPU_STATE MPU_CheckChip(void)
    {
        uint8_t check;

        HAL_I2C_Mem_Read(i2c,MPU_ADD,0x75,1,&check,1,TimeOut);
        if (check == 0x68)
            return MPU_OK;
        return MPU_ERROR;
    }

    MPU_STATE IMUupdate(void)
    {
        if(this->Get_Acce_AllAXIS_RawData() != MPU_OK)
            return MPU_ERROR;
        if(this->Get_Gyro_AllAXIS_RawData()!= MPU_OK)
            return MPU_ERROR;

        double gx = getGyroXdata();
        double gy = getGyroYdata();
        double gz = getGyroZdata();
        double ax = getAcceXdata();
        double ay = getAcceYdata();
        double az = getAcceZdata();

        double   norm;
        double  vx, vy, vz;
        double  ex, ey, ez;

        norm = sqrt(ax*ax + ay*ay + az*az);
        ax = ax / norm;
        ay = ay / norm;
        az = az / norm;

        vx = 2*(q1*q3 - q0*q2);
        vy = 2*(q0*q1 + q2*q3);
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

        ex = (ay*vz - az*vy);
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);

        exInt = exInt + ex*Ki;
        eyInt = eyInt + ey*Ki;
        ezInt = ezInt + ez*Ki;

        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt;

        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
        q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
        q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
        q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 / norm;
        q1 = q1 / norm;
        q2 = q2 / norm;
        q3 = q3 / norm;

        data.Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
        data.Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;
        data.Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;

        return MPU_OK;
    }
};

#endif //ONEBLOW_FLC_HAL_MPU60X0_H
